#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include <iostream>
#include <string>
#include <sstream>
#include <std_msgs/Int16.h>
#include <std_msgs/Int32.h>
#include <std_msgs/Int8.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Empty.h>
#include <stdlib.h>
#include <cstdio>
#include <cmath>
#include <collect_pkg/ESC_RAW_DATA.h>
#include <collect_pkg/ESC_DATA.h>

const int CMD_OFFSET = 1520;
const double C2PI = (2 * 3.1415926) / 16384;
const double PI = 3.1415926;

// 函数功能：将数据经由串口发送出去
// 入口参数1：[serial::Serial &ser]：     串口类名称
// 入口参数2：[std::string &serial_msg]:  要通过串口发送出去的字符串
int serial_write(serial::Serial &ser, std::string &serial_msg)
{
    ser.write(serial_msg);
    return 0;
}

// 函数功能：将从串口接收到的数据保存到数组中
// 入口参数1：[serial::Serial &ser]：     串口类名称
// 入口参数2：[std::string &serial_msg]:  从串口读取的字符串
int serial_read(serial::Serial &ser, std::string &serial_msg)
{
    serial_msg = ser.read(ser.available());
    return 0;
}

// CRC8
uint8_t updateCrc8(uint8_t crc, uint8_t crc_seed)
{
    uint8_t crc_u = crc ^ crc_seed;

    for (int i = 0; i < 8; ++i)
    {
        crc_u = (crc_u & 0x80) ? 0x7 ^ (crc_u << 1) : (crc_u << 1);
    }

    return crc_u;
}

// CRC8校验
uint8_t crc8(const uint8_t *buf, uint8_t len)
{
    uint8_t crc = 0;

    for (int i = 0; i < len; ++i)
    {
        crc = updateCrc8(buf[i], crc);
    }

    return crc;
}

// 字符转成16进制，a1为数组具体的一位。
std::string Char2Hex(char a1)
{
    std::string str = "";
    int n = a1; // n为a1的ASCII码
    for (int i = 2 * sizeof(char) - 1; i >= 0; i--)
    {
        str += "0123456789ABCDEF"[((n >> i * 4) & 0xF)];
    }
    return str;
}

int main(int argc, char **argv)
{

    setlocale(LC_CTYPE, "zh_CN.utf8"); // 设置编码，防止中文乱码

    // 初始化，节点名为serial_publisher
    ros::init(argc, argv, "serial_publisher");
    // 创建句柄seuNB，用于管理资源
    ros::NodeHandle nh;
    // 裸数据
    ros::Publisher esc_raw_data_pub =nh.advertise<collect_pkg::ESC_RAW_DATA>("esc_raw_data", 50);
    collect_pkg::ESC_RAW_DATA esc_raw_data;
    // 处理后数据
    ros::Publisher esc_data_pub =nh.advertise<collect_pkg::ESC_DATA>("esc_data", 50);
    collect_pkg::ESC_DATA esc_data;

    // 实例化一个serial类
    serial::Serial ser;
    // 初始化串口相关设置
    ser.setPort("/dev/ttyUSB0");                               // 设置打开的串口名称:这里打开一个虚拟串口
    ser.setBaudrate(115200);                                   // 设置串口的波特率
    serial::Timeout to = serial::Timeout::simpleTimeout(1000); // 创建timeout
    ser.setTimeout(to);                                        // 设置串口的timeout

    // 打开串口
    try
    {
        ser.open(); // 打开串口
    }

    catch (const std::exception &e)
    {
        ROS_ERROR_STREAM("Unable to open port."); // 打开串口失败，打印日志信息，然后结束程序
        return -1;
    }

    // 判断串口是否成功打开
    if (ser.isOpen())
    {
        ROS_INFO_STREAM("Serial Port is opened.\n"); // 成功打开串口，打印日志信息
    }
    else
    {
        return -1; // 打开串口失败，打印日志信息，然后结束程序
    }

    ros::Rate loop_rate(50); // 指定循环频率50
    while (ros::ok())
    {
        // 将数据传入到serial_msg1中
        // 获取缓冲区内的字节数
        size_t n = ser.available();
        uint8_t buffer[1024];
        if (n != 0)
        {
            uint8_t buffer[1024];
            n = ser.read(buffer, n);
            unsigned int crcCode;
            uint8_t number1[26] = {
                buffer[0], buffer[1],   
                buffer[2], buffer[3],   // 编码器位置
                buffer[4], buffer[5],   // 转速计算
                buffer[6], buffer[7],   // 相位计算
                buffer[8], buffer[9],   // 幅值计算
                buffer[10], buffer[11],   // 油门指令
                buffer[12], buffer[13],   // 正弦相位
                buffer[14], buffer[15],   // 正弦幅度
                buffer[16], buffer[17], // 电压
                buffer[18], buffer[19], // 电流
                buffer[20], buffer[21], // 功耗
                buffer[22], buffer[23], // 转速
                buffer[24]            //温度

            };
            int head0 = buffer[0];
            int head1 = buffer[1];
            if(head0 == 170 | head1 ==85){
            unsigned int number2[2] = {buffer[25]}; // crc8校验
            uint8_t  *ptr = NULL;
            ptr = number1;
            crcCode = crc8(ptr, 25);
            number1[26] = crcCode;

            int Calculated_value = crcCode;
            int Receive_value = number2[0];
            ROS_INFO_STREAM("Calculated_value"<<Calculated_value);
            ROS_INFO_STREAM("Receive_value"<<Receive_value);
            
            int encoder_orginal = buffer[3] << 8 | buffer[2]; 
            int calculate_speed_orginal = buffer[5] << 8 | buffer[4];     
            int calculate_phase_orginal = buffer[7] << 8 | buffer[6];
            int calculate_amplitude_orginal = buffer[9] << 8 | buffer[8];
            int throttle_orginal = buffer[11] << 8 | buffer[10];
            int x_orginal = buffer[13] << 8 | buffer[12];
            int y_orginal = buffer[15] << 8 | buffer[14];
            int voltage_orginal = buffer[17] << 8 | buffer[16];
            int current_orginal = buffer[19] << 8 | buffer[18];
            int power_consumption_orginal = buffer[21] << 8 | buffer[20];
            int rotational_speed_orginal = buffer[23] << 8 | buffer[22];
            int temperature_orginal = buffer[24];

            if (Calculated_value == Receive_value)
            {
                ROS_INFO("开始发布");
                esc_raw_data.position_encoder = encoder_orginal;
                esc_raw_data.speed_motor_pulse = calculate_speed_orginal;
                esc_raw_data.speed_encoder = rotational_speed_orginal;
                esc_raw_data.throttle_stick = throttle_orginal;
                esc_raw_data.direction_x_stick = x_orginal;
                esc_raw_data.direction_y_stick = y_orginal;
                esc_raw_data.phase_cmd = calculate_phase_orginal;
                esc_raw_data.amplitude_cmd = calculate_amplitude_orginal;
                esc_raw_data.voltage = voltage_orginal;
                esc_raw_data.current = current_orginal;
                esc_raw_data.power_consumption = power_consumption_orginal;
                esc_raw_data.temperature = temperature_orginal;
                //发布裸数据
                esc_raw_data_pub.publish(esc_raw_data);
                ROS_INFO_STREAM("裸数据如下:position_encoder:"<<esc_raw_data.position_encoder<<"speed_motor_pulse:"<<esc_raw_data.speed_motor_pulse);
                ROS_INFO_STREAM("speed_encoder:"<<esc_raw_data.speed_encoder<<"throttle_stick:"<<esc_raw_data.throttle_stick);
                ROS_INFO_STREAM("direction_x_stick:"<<esc_raw_data.direction_x_stick<<"direction_x_stick:"<<esc_raw_data.direction_y_stick);
                ROS_INFO_STREAM("phase_cmd:"<<esc_raw_data.phase_cmd<<"amplitude_cmd :"<<esc_raw_data.amplitude_cmd);
                ROS_INFO_STREAM("voltage:"<<esc_raw_data.voltage<<"current:"<<esc_raw_data.current);
                ROS_INFO_STREAM("power_consumption:"<<esc_raw_data.power_consumption<<"temperature :"<<esc_raw_data.temperature);
                //处理数据
                // 编码器位置数据。编码器范围0-16383对应（0-360度）
                double encoder = encoder_orginal * 360 / 16384.0
                ;
                esc_data.position_encoder = encoder;
                
                // 油门数据（原始数*2-2100）得到小于200就等于零
                if (throttle_orginal * 2 - 2100 < 200 || throttle_orginal * 2 - 2100 == 200)
                {
                    esc_data.throttle_stick  = 0;
                }
                else
                {
                    esc_data.throttle_stick  = throttle_orginal * 2 - 2100;
                }
                
                // 计算相位
                int x = x_orginal - CMD_OFFSET;
                int y = y_orginal - CMD_OFFSET;

                double phase = atan2(y, x) * 360.0 / (2 * PI);
                // 计算幅值
                double amplitude = sqrt(x * x + y * y) / 1000.0;
                if (amplitude < 0.05)
                {
                    amplitude = 0.0;
                    phase = 0.0;
                }
                esc_data.phase_cmd = phase;
                esc_data.amplitude_cmd = amplitude;
                esc_data.voltage = voltage_orginal / 1000.0;
                esc_data.current = current_orginal / 10000.0;
                esc_data.power_consumption = power_consumption_orginal / 1000.0;
                esc_data.speed_encoder= rotational_speed_orginal / 100.0;
                esc_data.temperature = temperature_orginal;
                esc_data_pub.publish(esc_data);
                ROS_INFO_STREAM("处理后数据如下:position_encoder:"<<esc_data.position_encoder<<"speed_motor_pulse:"<<esc_data.speed_motor_pulse);
                ROS_INFO_STREAM("speed_encoder:"<<esc_data.speed_encoder<<"throttle_stick:"<<esc_data.throttle_stick);
                ROS_INFO_STREAM("phase_cmd:"<<esc_data.phase_cmd<<"amplitude_cmd :"<<esc_data.amplitude_cmd);
                ROS_INFO_STREAM("voltage:"<<esc_data.voltage<<"current:"<<esc_data.current);
                ROS_INFO_STREAM("power_consumption:"<<esc_data.power_consumption<<"temperature :"<<esc_data.temperature);
            }else{
                ROS_INFO("校验码出错");
                return -1; // 打开串口失败，打印日志信息，然后结束程序
            }}
            else{
                ROS_INFO("包头解析错误");
                ROS_INFO_STREAM("head0"<<head0);
                ROS_INFO_STREAM("head1"<<head1);

            }

            loop_rate.sleep();
        }
    }
    ser.close();
    return 0;
}